/*
 *  Player Java Client 2 - LaserInterface.java
 *  Copyright (C) 2002-2006 Radu Bogdan Rusu, Maxim Batalin
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * $Id: LaserInterface.java,v 1.4 2006/03/10 19:04:59 veedee Exp $
 *
 */
package javaclient2;

import java.io.IOException;

import javaclient2.xdr.OncRpcException;
import javaclient2.xdr.XdrBufferDecodingStream;
import javaclient2.xdr.XdrBufferEncodingStream;

import javaclient2.structures.PlayerMsgHdr;
import javaclient2.structures.PlayerPose;
import javaclient2.structures.PlayerBbox;
import javaclient2.structures.laser.PlayerLaserData;
import javaclient2.structures.laser.PlayerLaserDataScanpose;
import javaclient2.structures.laser.PlayerLaserConfig;
import javaclient2.structures.laser.PlayerLaserGeom;


/**
 * The laser interface provides access to a single-origin scanning range 
 * sensor, such as a SICK laser range-finder (e.g., sicklms200).
 * <br><br>
 * Devices supporting the laser interface can be configured to scan at 
 * different angles and resolutions. As such, the data returned by the 
 * laser interface can take different forms. To make interpretation of 
 * the data simple, the laser data packet contains some extra fields 
 * before the actual range data. These fields tell the client the starting 
 * and ending angles of the scan, the angular resolution of the scan, and 
 * the number of range readings included. Scans proceed counterclockwise 
 * about the laser (0 degrees is forward). The laser can return a maximum of 
 * PLAYER_LASER_MAX_SAMPLES readings; this limits the valid combinations of 
 * scan width and angular resolution.
 * @author Radu Bogdan Rusu, Maxim Batalin
 * @version
 * <ul>
 *      <li>v2.0 - Player 2.0 supported
 * </ul>
 */
public class LaserInterface extends PlayerDevice {
	
    private static final boolean isDebugging = PlayerClient.isDebugging;
    
    private PlayerLaserData         pldata;
    private boolean                 readyPldata     = false;
    private PlayerLaserDataScanpose pldatascan;
    private boolean                 readyPldatascan = false;
    private PlayerLaserConfig 		plconfig;
    private boolean                 readyPlconfig   = false;
    private PlayerLaserGeom 		plgeom;
    private boolean                 readyPlgeom     = false;
    
    /**
     * Constructor for LaserInterface.
     * @param pc a reference to the PlayerClient object
     */
    public LaserInterface (PlayerClient pc) { super (pc); }

    private synchronized PlayerLaserData readLaserData () {
    	PlayerLaserData pld = new PlayerLaserData ();
    	try {
    		// Buffer for reading min/max_angle, resolution, max_range, ranges_count
    		byte[] buffer = new byte[20];
    		// Read min/max_angle, resolution, max_range, ranges_count
    		is.readFully (buffer, 0, 20);
    		
    		// Begin decoding the XDR buffer
    		XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
    		xdr.beginDecoding ();
    		pld.setMin_angle    (xdr.xdrDecodeFloat ());
    		pld.setMax_angle    (xdr.xdrDecodeFloat ());
    		pld.setResolution   (xdr.xdrDecodeFloat ());
    		pld.setMax_range    (xdr.xdrDecodeFloat ());
    		int rangesCount = xdr.xdrDecodeInt ();
    		xdr.endDecoding   ();
    		xdr.close ();
    		
    		// Buffer for reading range values
    		buffer = new byte[PLAYER_LASER_MAX_SAMPLES * 4 + 4];
    		// Read range values
    		is.readFully (buffer, 0, rangesCount * 4 + 4);
    		xdr = new XdrBufferDecodingStream (buffer);
    		xdr.beginDecoding ();
    		pld.setRanges (xdr.xdrDecodeFloatVector ());
    		xdr.endDecoding   ();
    		xdr.close ();
    		
    		pld.setRanges_count (rangesCount);
    		
    		// Buffer for intensity_count
    		buffer = new byte[8];
    		// Read intensity_count
    		is.readFully (buffer, 0, 8);
    		
    		// Begin decoding the XDR buffer
    		xdr = new XdrBufferDecodingStream (buffer);
    		xdr.beginDecoding ();
    		int intensityCount = xdr.xdrDecodeInt ();
    		xdr.endDecoding   ();
    		xdr.close ();
    		
    		// Buffer for reading intensity values (non XDR)
    		buffer = new byte[PLAYER_LASER_MAX_SAMPLES];
    		// Read intensity values
    		is.readFully (buffer, 0, intensityCount);
    		pld.setIntensity_count (intensityCount);
    		pld.setIntensity       (buffer);
    		
    		// Take care of the residual zero bytes
    		if ((intensityCount % 4) != 0)
    			is.readFully (buffer, 0, 4 - (intensityCount % 4));
    		
    		
    		// Buffer for ID
    		buffer = new byte[4];
    		// Read ID
    		is.readFully (buffer, 0, 4);
    		xdr = new XdrBufferDecodingStream (buffer);
    		xdr.beginDecoding ();
    		pld.setId        (xdr.xdrDecodeInt ());
    		xdr.endDecoding   ();
    		xdr.close ();
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Laser] : Error reading laser data: " + 
        				e.toString(), e);
        } catch (OncRpcException e) {
        	throw new PlayerException 
        		("[Laser] : Error while XDR-decoding laser data: " + 
        				e.toString(), e);
        }
    	return pld;
    }
    
    /**
     * Read the laser data packet.
     */
    public synchronized void readData (PlayerMsgHdr header) {
        try {
        	switch (header.getSubtype ()) {
        		case PLAYER_LASER_DATA_SCAN: {
        			pldata = readLaserData ();
        	    	readyPldata = true;
        			break;
        		}
        		case PLAYER_LASER_DATA_SCANPOSE: {
        			PlayerLaserData pld = readLaserData ();
        			PlayerPose pp = new PlayerPose ();
        			
        			// Buffer for reading pose
        			byte[] buffer = new byte[12];
        			// Read pose
        			is.readFully (buffer, 0, 12);
        			
        			// Begin decoding the XDR buffer
        			XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
        			xdr.beginDecoding ();
        			pp.setPx (xdr.xdrDecodeFloat ());
        			pp.setPy (xdr.xdrDecodeFloat ());
        			pp.setPa (xdr.xdrDecodeFloat ());
        			xdr.endDecoding   ();
        			xdr.close ();
        			
        			pldatascan = new PlayerLaserDataScanpose ();
        			
        			pldatascan.setScan (pld);
        			pldatascan.setPose (pp);
        			
        			readyPldatascan = true;
        			break;
        		}
        	}
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Laser] : Error reading payload: " + 
        				e.toString(), e);
        } catch (OncRpcException e) {
        	throw new PlayerException 
        		("[Laser] : Error while XDR-decoding payload: " + 
        				e.toString(), e);
        }
    }
    
    /**
     * Get the laser data.
     * @return an object of type PlayerLaserData containing the laser data 
     */
    public PlayerLaserData getData () { return this.pldata; }
    
    /**
     * Check if data is available.
     * @return true if ready, false if not ready 
     */
    public boolean isDataReady () {
        if (readyPldata) {
        	readyPldata = false;
            return true;
        }
        return false;
    }

   /** 
    * Get a laser scan with a pose that indicates the (possibly estimated) 
    * pose of the laser when the scan was taken.
    * @return an object of type PlayerLaserDataScanpose containing the laser data 
    */
   public PlayerLaserDataScanpose getDataScanPose () { return this.pldatascan; }
   
   /**
    * Check if Scanpose data is available.
    * @return true if ready, false if not ready 
    */
   public boolean isDataScanposeReady () {
       if (readyPldatascan) {
    	   readyPldatascan = false;
           return true;
       }
       return false;
   }
    
    /**
     * Configuration request: Get geometry.
     * <br><br>
     * The laser geometry (position and size) can be queried.
     * <br><br>
     * See the player_laser_geom structure from player.h
     */
    public void queryGeometry () {
    	try {
            sendHeader (PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_GET_GEOM, 0);
            os.flush ();
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Laser] : Couldn't request PLAYER_LASER_REQ_GET_GEOM: " + 
        				e.toString(), e);
        }
    }
    
    /**
     * Configuration request: Set scan properties.
     * <br><br>
     * The scan configuration (resolution, aperture, etc) can be queried 
     * by sending a null PLAYER_LASER_REQ_GET_CONFIG request and modified 
     * by sending a PLAYER_LASER_REQ_SET_CONFIG request. In either case, 
     * the current configuration (after attempting any requested modification) 
     * will be returned in the response.
     * @param plc a PlayerLaserConfig structure holding the laser configuration data
     */
    public void setScanProperties (PlayerLaserConfig plc) {
        try {
        	sendHeader (PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_SET_CONFIG, 24);
        	XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (24);
        	xdr.beginEncoding (null, 0);
        	xdr.xdrEncodeFloat (plc.getMin_angle  ());
        	xdr.xdrEncodeFloat (plc.getMax_angle  ());
        	xdr.xdrEncodeFloat (plc.getResolution ());
        	xdr.xdrEncodeFloat (plc.getMax_range  ());
        	xdr.xdrEncodeFloat (plc.getRange_res  ());
        	xdr.xdrEncodeByte  (plc.getIntensity  ());
        	xdr.endEncoding ();
        	os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
        	xdr.close ();
        	os.flush ();
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Laser] : Couldn't request PLAYER_LASER_REQ_SET_CONFIG: " + 
        				e.toString(), e);
        } catch (OncRpcException e) {
        	throw new PlayerException 
        		("[Laser] : Error while XDR-encoding SET_CONFIG request: " + 
        				e.toString(), e);
        }
    }

    /**
     * Configuration request: Get scan properties.
     * <br><br>
     * The scan configuration (resolution, aperture, etc) can be queried 
     * by sending a null PLAYER_LASER_REQ_GET_CONFIG request and modified 
     * by sending a PLAYER_LASER_REQ_SET_CONFIG request. In either case, 
     * the current configuration (after attempting any requested modification) 
     * will be returned in the response.
     */
    public void getScanProperties () {
    	try {
            sendHeader (PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_GET_CONFIG, 0);
            os.flush ();
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Laser] : Couldn't request PLAYER_LASER_REQ_GET_CONFIG: " + 
        				e.toString(), e);
        }
    }
    
    /**
     * Configuration request: Turn power on/off.
     * @param value 0 to turn laser off, 1 to turn laser on 
     */
    public void setPower (int value) {
        try {
        	sendHeader (PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_POWER, 4);
        	XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (4);
        	xdr.beginEncoding (null, 0);
        	xdr.xdrEncodeByte ((byte)value);
        	xdr.endEncoding ();
        	os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
        	xdr.close ();
        	os.flush ();
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Laser] : Couldn't request PLAYER_LASER_REQ_POWER: " + 
        				e.toString(), e);
        } catch (OncRpcException e) {
        	throw new PlayerException 
        		("[Laser] : Error while XDR-encoding POWER request: " + 
        				e.toString(), e);
        }
    }
    
    /**
     * Handle acknowledgement response messages (threaded mode).
     * @param header Player headerd
     */
    public void handleResponse (PlayerMsgHdr header) {
        try {
            switch (header.getSubtype ()) {
                case PLAYER_LASER_REQ_GET_GEOM: {
                	// Buffer for reading pose and size
                	byte[] buffer = new byte[12+8];
                	// Read pose and size
                	is.readFully (buffer, 0, 12+8);
                	
                	PlayerPose pp = new PlayerPose ();
                	PlayerBbox pb = new PlayerBbox ();
                	
                	// Begin decoding the XDR buffer
                	XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
                	xdr.beginDecoding ();
                	pp.setPx (xdr.xdrDecodeFloat ());
                	pp.setPy (xdr.xdrDecodeFloat ());
                	pp.setPa (xdr.xdrDecodeFloat ());
                	pb.setSw (xdr.xdrDecodeFloat ());
                	pb.setSl (xdr.xdrDecodeFloat ());
                	xdr.endDecoding   ();
                	xdr.close ();
                	
                	plgeom = new PlayerLaserGeom ();
                	plgeom.setPose (pp);
                	plgeom.setSize (pb);
                	
                	readyPlgeom = true;
                	break;
                }
                case PLAYER_LASER_REQ_SET_CONFIG: {
                	break;
                }
                case PLAYER_LASER_REQ_GET_CONFIG: {
                	// Buffer for reading laser configuration data
                	byte[] buffer = new byte[24];
                	// Read laser configuration data
                	is.readFully (buffer, 0, 24);
                	
                	plconfig = new PlayerLaserConfig ();
                	
                	// Begin decoding the XDR buffer
                	XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
                	xdr.beginDecoding ();
                	plconfig.setMin_angle  (xdr.xdrDecodeFloat ());
                	plconfig.setMax_angle  (xdr.xdrDecodeFloat ());
                	plconfig.setResolution (xdr.xdrDecodeFloat ());
                	plconfig.setMax_range  (xdr.xdrDecodeFloat ());
                	plconfig.setRange_res  (xdr.xdrDecodeFloat ());
                	plconfig.setIntensity  (xdr.xdrDecodeByte  ());
                	xdr.endDecoding   ();
                	xdr.close ();
                	
                	readyPlconfig = true;
                	break;
                }
                case PLAYER_LASER_REQ_POWER: {
                	break;
                }
                default:{
                	if (isDebugging)
                		System.err.println ("[Laser]Debug] : " +
                				"Unexpected response " + header.getSubtype () + 
                				" of size = " + header.getSize ());
                    break;
                }
            }
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Laser] : Error reading payload: " + 
        				e.toString(), e);
        } catch (OncRpcException e) {
        	throw new PlayerException 
        		("[Laser] : Error while XDR-decoding payload: " + 
        				e.toString(), e);
        }
    }
    
    /**
     * Get the laser geometry after a PLAYER_LASER_REQ_GET_GEOM request.
     * @return an object of PlayerLaserGeom type containing the laser geometry
     * @see #isReadyPlgeom()
     */
    public PlayerLaserGeom   getPlayerLaserGeom   () { return plgeom; }
    
    /**
     * Get the laser configuration after a PLAYER_LASER_REQ_GET_CONFIG request.
     * @return an object of PlayerLaserConfig type containing the laser configuration
     * @see #isReadyPlconfig()
     */
    public PlayerLaserConfig getPlayerLaserConfig () { return plconfig; }
    
    /**
     * Check if the geometry data is available.
     * @return true if ready, false if not ready
     * @see #getPlayerLaserGeom()
     */
    public boolean isReadyPlgeom () {
        if (readyPlgeom) {
        	readyPlgeom = false;
            return true;
        }
        return false;
    }
    
    /**
     * Check if the configuration data is available.
     * @return true if ready, false if not ready
     * @see #getPlayerLaserConfig()
     */
    public boolean isReadyPlconfig () {
        if (readyPlconfig) {
        	readyPlconfig = false;
            return true;
        }
        return false;
    }
}
